
"""
工具类模块：提供着陆系统所需的各种计算工具
包含：距离计算、方位角计算、垂直速度计算等通用功能
"""
import math
import logging
from landing.config import INTEGRATED_CONFIG
from landing.evaluator import  EnhancedLandingEvaluator


class RewardIntegrator:
    def __init__(self, config=INTEGRATED_CONFIG):
        self.config = config
        self.landing_evaluator = EnhancedLandingEvaluator(config)
        self.logger = logging.getLogger(__name__)
        self.current_phase = None

    def calculate_reward(self, state_data, is_on_ground):
        """计算综合奖励"""
        if is_on_ground:
            # 使用原有的地面奖励逻辑
            return self._calculate_ground_reward(state_data)

        # 检测当前高度和位置来决定是否进入着陆阶段
        if self._is_in_landing_phase(state_data):
            # 使用着陆评估器计算奖励
            landing_rewards, phase = self.landing_evaluator.evaluate_landing(state_data)
            self.current_phase = phase
            return landing_rewards['total']
        else:
            # 使用原有的空中奖励逻辑
            return self._calculate_air_reward(state_data)

    def _is_in_landing_phase(self, state_data):
        """判断是否进入着陸阶段"""
        alt = state_data[32]  # 当前高度
        target_alt = self.config['TAKEOFF']['TARGET']['ALT']

        # 如果高度低于目标高度的20%，认为进入着陆阶段
        return alt < (target_alt * 0.2)

    def _calculate_ground_reward(self, state_data):
        """计算地面阶段奖励"""
        # 保持原有的地面奖励逻辑
        return 0

    def _calculate_air_reward(self, state_data):
        """计算空中阶段奖励"""
        # 保持原有的空中奖励逻辑
        return 0

    def get_current_phase(self):
        """获取当前飞行阶段"""
        return self.current_phase


